#include "ros_robot_control.h"

/************************************************************************
函数名称：   void *client_fun(void *arg)
函数功能：   进程id返回函数。
函数参数：   已连接套接字
函数返回：   无
************************************************************************/
pid_t getProcessPidByName(const char *proc_name)
{
     FILE *fp;
     char buf[100];
     char cmd[200] = {'\0'};
     pid_t pid = -1;
     sprintf(cmd, "pidof %s", proc_name);
     if((fp = popen(cmd, "r")) != NULL)
     {
         if(fgets(buf, 255, fp) != NULL)
         {
             pid = atoi(buf);
         }
     }
     //printf("pid = %d\n", pid);
     pclose(fp);
     return pid;
}


/************************************************************************
函数名称：   void kill_nodel(const char *node)
函数功能：   关闭除某些特定节点
函数参数：   无
函数返回：   无
************************************************************************/
void kill_nodel(const char *node)
{
    FILE *fp;
    char cmd[200] = {'\0'};
    //关闭除某些特定节点
    sprintf(cmd, "rosnode kill %s", node);
    if((fp = popen(cmd, "r")) != NULL)
    {
         
    }
    pclose(fp);
}


/************************************************************************
函数名称：   run_move_base
函数功能：   move_base包打开函数以及log保存进程
函数参数：   已连接套接字
函数返回：   无
************************************************************************/
void *run_move_base(void *arg)
{

    std::string str="roslaunch " + file_pwd + "/zkwl_robot_start/launch/app_navigation.launch";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    int status = system(p);
    if(status < 0)
    {
        ROS_ERROR("movebase error: %s", strerror(errno));
        pthread_detach(thread_move_base); // 线程分离，结束时自动回收资源
    }
}

/************************************************************************
函数名称：   run_zkwl_robot
函数功能：   run_zkwl_robot包打开函数以及log保存进程
函数参数：   已连接套接字
函数返回：   无
************************************************************************/
void *run_zkwl_robot(void *arg)
{
    std::string str="roslaunch " + file_pwd + "/zkwl_robot_start/launch/zkwl_robot_start.launch";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    int status = system(p);
    if(status < 0)
    {
        ROS_ERROR("zkwlrobot error: %s", strerror(errno));
        pthread_detach(thread_zkwl_robot); // 线程分离，结束时自动回收资源
    }
}

/************************************************************************
函数名称：   run_slam_karto
函数功能：   run_slam_karto包打开函数以及log保存进程
函数参数：   已连接套接字
函数返回：   无
************************************************************************/
void *run_slam_karto(void *arg)
{
    std::string str="roslaunch " + file_pwd + "/zkwl_robot_start/launch/karto_slam.launch";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    int status = system(p);
    if(status < 0)
    {
        ROS_ERROR("zkwlrobot error: %s", strerror(errno));
        pthread_detach(thread_slam_karto); // 线程分离，结束时自动回收资源
    }
}

/************************************************************************
函数名称：   run_slam_karto
函数功能：   run_slam_karto包打开函数以及log保存进程
函数参数：   已连接套接字
函数返回：   无
************************************************************************/
void *run_save_map(void *arg)
{
    std::string str="bash " + file_pwd + "/zkwl_robot_start/sh/2d_save_map.sh";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    int status = system(p);
    if(status < 0)
    {
        ROS_ERROR("zkwlrobot error: %s", strerror(errno));
        pthread_detach(thread_save_map); // 线程分离，结束时自动回收资源
    }
}

/************************************************************************
函数名称：   robot_control_Callback
函数功能：   robot_control_Callback
函数参数：   已连接套接字
函数返回：   无
************************************************************************/
void robot_control_Callback(const std_msgs::String::ConstPtr& msg)
{
   
    static unsigned int domain;
    

    if (msg->data == "start_zkwl")
    {
        domain = 1;
    }
    else if(msg->data == "kill_zkwl")
    {
        domain = 2;
    }
    else if(msg->data == "start_nav")
    {
        domain = 3;
    }
    else if(msg->data == "kill_nav")
    {
        domain = 4;
    }
    else if(msg->data == "start_slam")
    {
        domain = 5;
    }
    else if(msg->data == "kill_slam")
    {
        domain = 6;
    }
    else if(msg->data == "save_map")
    {
        domain = 7;
    }
    else if(msg->data == "clear_cruisepoint")
    {
        domain = 8;
    }
    //find 
    else if(msg->data.find("start_move_base") != std::string::npos)
    {   
        //截取字符串
        move_base_timer = atoi(msg->data.substr(16).c_str());
        //set timer
        movebase_timer.setPeriod(ros::Duration(move_base_timer),true);

        cout <<"timer:"<< move_base_timer << endl;
        domain = 9;
    }
    else if(msg->data == "stop_move_base")
    {
        domain = 10;
    }
    else if(msg->data == "set_power_point")
    {
        domain = 11;
    }
    else if(msg->data == "go_power_point")
    {
        domain = 12;
    }
     //go to power 
    //set_go_power:21:24:20:0
    else if(msg->data.find("set_go_power") != std::string::npos)
    {   
        //截取字符串
        int temp_index=0,temp1_index = 0;

        temp1_index = msg->data.find(":",temp_index)+1;
        temp_index  = msg->data.find(":",temp1_index)+1;
        min_power = msg->data.substr(temp1_index,temp_index-temp1_index-1).c_str();

        temp1_index = msg->data.find(":",temp_index)+1;
        max_power = msg->data.substr(temp_index,temp1_index-temp_index-1).c_str();

        temp_index = msg->data.find(":",temp1_index)+1;
        go_power = msg->data.substr(temp1_index,temp_index-temp1_index-1).c_str();

        check_power = msg->data.substr(temp_index).c_str();
    
        ROS_WARN("set go charge min:%s  max:%s  go:%s  check:%s",min_power.c_str(),max_power.c_str(),go_power.c_str(),check_power.c_str());
        cout << "--------------------------------------------------------------------" << endl;
        domain = 13;
    }
    else if(msg->data == "cancel_go_charge")
    {
        domain = 14;
    }
    else
    {
        domain = 0;
    }
    
    ROS_WARN("RXCMD:%d\n",domain );
    cout << "--------------------------------------------------------------------" << endl;
    switch(domain)
    {
        case 0:
        {
            break;
        }
        //zkwl_robot_node
        case 1:
        {
            //由于同一个进程内的所有线程共享内存和变量，因此在传递参数时需作特殊处理，值传递。
            int ret = pthread_create(&thread_zkwl_robot, NULL, run_zkwl_robot, (void *)domain);  //创建线程
            pthread_detach(thread_zkwl_robot); // 线程分离，结束时自动回收资源
            break;
        }
        case 2:
        {
            pthread_detach(thread_zkwl_robot); // 线程分离，结束时自动回收资源
            kill_nodel("/publish_imu");
            kill_nodel("/publish_mec");
            kill_nodel("/publish_odom");
            break;
        }
        case 3:
        {
            //由于同一个进程内的所有线程共享内存和变量，因此在传递参数时需作特殊处理，值传递。
            int ret = pthread_create(&thread_move_base, NULL, run_move_base, (void *)domain);  //创建线程
            pthread_detach(thread_move_base); // 线程分离，结束时自动回收资源
            break;
        }
        case 4:
        {
            pthread_detach(thread_move_base); // 线程分离，结束时自动回收资源
            kill_nodel("/amcl");
            kill_nodel("/move_base");
            kill_nodel("/robot_pose_ekf");
            kill_nodel("/map_server_for_test");
            break;
        }
        case 5:
        {
            //由于同一个进程内的所有线程共享内存和变量，因此在传递参数时需作特殊处理，值传递。
            int ret = pthread_create(&thread_slam_karto, NULL, run_slam_karto, (void *)domain);  //创建线程
            pthread_detach(thread_slam_karto); // 线程分离，结束时自动回收资源
            break;
        }
        case 6:
        {
            pthread_detach(thread_slam_karto); // 线程分离，结束时自动回收资源
            kill_nodel("/slam_karto");
            kill_nodel("/robot_pose_ekf");
            break;
        }
        case 7:
        {
            
            //由于同一个进程内的所有线程共享内存和变量，因此在传递参数时需作特殊处理，值传递。
            int ret = pthread_create(&thread_save_map, NULL, run_save_map, (void *)domain);  //创建线程
            pthread_detach(thread_save_map); // 线程分离，结束时自动回收资源
            break;
        }
        case 8:
        {
            clear_cruisepoint();
            break;
        }
        case 9:
        {
            //开启巡航
            if(move_base_status == 0 && mark_cruise_poit_array.markers.size())
            {
                double x,y,z,w;
                x = mark_cruise_poit_array.markers.at(move_base_status).pose.position.x;
                y = mark_cruise_poit_array.markers.at(move_base_status).pose.position.y;
                z = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.z;
                w = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.w;
                set_goal("map",x,y,z,w);
                move_base_status = 1;
                go_charge_status = 0;
                go_charge_error_cnt = 0;
            }
            else
            {
                ROS_WARN("have no cruise point: %d %d\n",move_base_status,mark_cruise_poit_array.markers.size());
            }
            break;
            
        }
        case 10:
        {
            //关闭巡航
            move_base_status = 0;
            //stop timer
            movebase_timer.stop();
            //
            move_base_cancel();
            break;
        }
        case 11:
        {
            
            //get tf pose
            update_pose();
            //回充点设置
            powerPointSet();
            break;
        }
        //去回充点
        case 12:
        {
            //
            goto_power_point();
            break;
        }

        //回充参数设置
        case 13:
        {
            //
            goto_power_save();
            break;
        }
        case 14:
        {
            move_base_cancel();
            charge_control(0);
            break;
        }
        
        default:break;
    }
}



//mark pub
void markshowandadd(const geometry_msgs::PoseStamped &pos)
{
    //save pose to txt
    Eigen::Vector3d pose_points;
    double pose_th = 0.0;
    tf::Quaternion q;

    tf::quaternionMsgToTF(pos.pose.orientation, q);
    pose_th = tf::getYaw(q);  //角度
    pose_points(0) = pos.pose.position.x;
    pose_points(1) = pos.pose.position.y;
    pose_points(2) = pose_th;
   
    std::string str= file_pwd + "/ros_robot_control/pose/pose.txt";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    std::ofstream out(p,std::ios::app);
    out<<fixed<<setprecision(2)<<pose_points(0)<<"\t\t"<<pose_points(1)<<"\t\t"<<pose_points(2)<<std::endl; 
    out.close();

    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    marker.header.frame_id = "map";
    marker.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    marker.ns = "basic_shapes";
    marker.id = mark_count++;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    marker.type = visualization_msgs::Marker::ARROW;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    marker.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header

    marker.pose = pos.pose;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    marker.scale.x = 0.7;
    marker.scale.y = 0.1;
    marker.scale.z = 0.2;

    // Set the color -- be sure to set alpha to something non-zero!
    marker.color.r = 0.0f;
    marker.color.g = 1.0f;
    marker.color.b = 0.0f;
    marker.color.a = 0.5;//透明

    //time 永久
    marker.lifetime = ros::Duration();



    markertext = marker;
    markertext.id = mark_count++;
    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    markertext.scale.x = 0.7;
    markertext.scale.y = 0.7;
    markertext.scale.z = 0.7;

    // Set the color -- be sure to set alpha to something non-zero!
    markertext.color.r = 1.0f;
    markertext.color.g = 0.0f;
    markertext.color.b = 0.0f;
    markertext.color.a = 1;//透明

    markertext.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

    stringstream text;
    text << mark_cruise_poit_array.markers.size()/2;
    markertext.text = text.str();

    mark_cruise_poit_array.markers.push_back(marker);
    mark_cruise_poit_array.markers.push_back(markertext);
    markarray_pub.publish(mark_cruise_poit_array);

}

//clear mark
void clear_cruisepoint()
{
    std::string str= file_pwd + "/ros_robot_control/pose/pose.txt";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*
    //clear pose.txt
    std::ofstream out(p,std::ios::out);
    out.close();

    //clear mark
    mark_count = 0;
    marker.action = visualization_msgs::Marker::DELETEALL;
    markertext.action = visualization_msgs::Marker::DELETEALL;
    mark_cruise_poit_array.markers.push_back(marker);
    mark_cruise_poit_array.markers.push_back(markertext);
    markarray_pub.publish(mark_cruise_poit_array);
    mark_cruise_poit_array.markers.clear();
}


void LoadImages(const std::string &trajectory, std::vector<std::string> &position_x)
{
    std::ifstream fTimes;
    // 数据流读取文件
    fTimes.open(trajectory.c_str());
    position_x.reserve(5000);

    while(!fTimes.eof())
    {
        std::string s;
        getline(fTimes,s);
        if(!s.empty())
        {
          position_x.push_back(s); 
        }
    }
}

//load markarray from pos.txt
void load_mark_frompostxt()
{
    std::vector<std::string> vposition;
    std::string x, y, z;

    std::string str= file_pwd + "/ros_robot_control/pose/pose.txt";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    LoadImages(p,vposition);
    //计算当前点在路径的位置
    for(int j=0;j<vposition.size();j++)
    {
        x = vposition[j].substr(0, 5); //substr就是截取字符，20指的是在第几个字符上，13从第20个数据向后截取的长度。
        y = vposition[j].substr(6, 11);
        z = vposition[j].substr(12, 17);
        geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(atof(z.c_str()));

         // Set the frame ID and timestamp.  See the TF tutorials for information on these.
        marker.header.frame_id = "map";
        marker.header.stamp = ros::Time::now();

        // Set the namespace and id for this marker.  This serves to create a unique ID
        // Any marker sent with the same namespace and id will overwrite the old one
        marker.ns = "basic_shapes";
        marker.id = mark_count++;

        // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
        marker.type = visualization_msgs::Marker::ARROW;

        // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
        marker.action = visualization_msgs::Marker::ADD;

        // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header

        marker.pose.position.x = atof(x.c_str());
        marker.pose.position.y = atof(y.c_str());

        marker.pose.orientation.x = goal_quat.x;
        marker.pose.orientation.y = goal_quat.y;
        marker.pose.orientation.z = goal_quat.z;
        marker.pose.orientation.w = goal_quat.w;

        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        marker.scale.x = 0.7;
        marker.scale.y = 0.1;
        marker.scale.z = 0.2;

        // Set the color -- be sure to set alpha to something non-zero!
        marker.color.r = 0.0f;
        marker.color.g = 1.0f;
        marker.color.b = 0.0f;
        marker.color.a = 0.5;//透明

        //time 永久
        marker.lifetime = ros::Duration();



        markertext = marker;
        markertext.id = mark_count++;
        // Set the scale of the marker -- 1x1x1 here means 1m on a side
        markertext.scale.x = 0.7;
        markertext.scale.y = 0.7;
        markertext.scale.z = 0.7;

        // Set the color -- be sure to set alpha to something non-zero!
        markertext.color.r = 1.0f;
        markertext.color.g = 0.0f;
        markertext.color.b = 0.0f;
        markertext.color.a = 1;//透明

        markertext.type = visualization_msgs::Marker::TEXT_VIEW_FACING;

        stringstream text;
        text << mark_cruise_poit_array.markers.size()/2;
        markertext.text = text.str();

        mark_cruise_poit_array.markers.push_back(marker);
        mark_cruise_poit_array.markers.push_back(markertext);
        
    }
    markarray_pub.publish(mark_cruise_poit_array);
     
}

//加载回充点从txt文件
void Load_power_markfromtxt(void)
{
    std::vector<std::string> vposition;
    std::string x, y, z;

    std::string str= file_pwd + "/ros_robot_control/pose/power_pose.txt";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    LoadImages(p,vposition);

    x = vposition[0].substr(0, 5); //substr就是截取字符，20指的是在第几个字符上，13从第20个数据向后截取的长度。
    y = vposition[0].substr(6, 11);
    z = vposition[0].substr(12, 17);
    geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(atof(z.c_str()));

    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    powerpointmark.header.frame_id = "map";
    powerpointmark.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    powerpointmark.ns = "basic_shapes";
    powerpointmark.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    powerpointmark.type = visualization_msgs::Marker::CUBE;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    powerpointmark.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header

    powerpointmark.pose.position.x = atof(x.c_str());
    powerpointmark.pose.position.y = atof(y.c_str());
    powerpointmark.pose.position.z = 0;
    powerpointmark.pose.orientation.x = 0.0;
    powerpointmark.pose.orientation.y = 0.0;
    powerpointmark.pose.orientation.z = goal_quat.z;
    powerpointmark.pose.orientation.w = goal_quat.w;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    powerpointmark.scale.x = 0.3;
    powerpointmark.scale.y = 0.5;
    powerpointmark.scale.z = 0.3;

    // Set the color -- be sure to set alpha to something non-zero!
    powerpointmark.color.r = 0.0f;
    powerpointmark.color.g = 1.0f;
    powerpointmark.color.b = 0.0f;
    powerpointmark.color.a = 1.0;

    //time 永久
    powerpointmark.lifetime = ros::Duration();

    powerpoint_pub.publish(powerpointmark);
}

//回充数据加载
void Load_goto_power_valuefromyaml(void)
{
    std::string fin = file_pwd + "/ros_robot_control/param/power.yaml";       //yaml文件所在的路径
    YAML::Node yamlConfig = YAML::LoadFile(fin);
    //int int_param = yamlConfig["int_param"].as<int>();
    min_power   = yamlConfig["min_power"].as<string>();
    max_power   = yamlConfig["max_power"].as<string>();
    go_power    = yamlConfig["go_power"].as<string>();
    check_power = yamlConfig["check_power"].as<string>();

    ROS_WARN("load power value success!");
    ROS_WARN("set go charge min:%s  max:%s  go:%s  check:%s",min_power.c_str(),max_power.c_str(),go_power.c_str(),check_power.c_str());
    cout << "--------------------------------------------------------------------" << endl;
}

//回充参数保存
void goto_power_save(void)
{
    std::string str= file_pwd + "/ros_robot_control/param/power.yaml";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*

    std::ofstream fout(p);

    YAML::Emitter out(fout);
    out << YAML::BeginMap;
    out << YAML::Key << "min_power";
    out << YAML::Value << min_power;

    out << YAML::Key << "max_power";
    out << YAML::Value << max_power;

    out << YAML::Key << "go_power";
    out << YAML::Value << go_power;

    out << YAML::Key << "check_power";
    out << YAML::Value << check_power;
    out << YAML::EndMap;
    
    ROS_WARN("save power value success!");
    cout << "--------------------------------------------------------------------" << endl;
}

//回充
void goto_power_point(void)
{
    //记录回充前的巡航点
    if(move_base_status != 0)
    {
        go_charge_temp_movebase_status = move_base_status;
        //关闭巡航
        move_base_status = 0;
    }
    
    //stop timer
    movebase_timer.stop();
    //开启回充
    go_charge_status = 1;
    move_base_cancel();
    set_goal("map",powerpointmark.pose.position.x,powerpointmark.pose.position.y,powerpointmark.pose.orientation.z,powerpointmark.pose.orientation.w);

    ROS_WARN("go to power!");
    cout << "--------------------------------------------------------------------" << endl;
    
}

//巡航点设置订阅
void cruisepointCallback(const geometry_msgs::PoseStamped &pos)
{
    markshowandadd(pos);
}

//导航结果回调函数
void movebase_resultCallback(const move_base_msgs::MoveBaseActionResult &result)
{
    double x,y,z,w;
    static unsigned char error = 0;

    //成功
    if(result.status.status == 3)
    {
        //错误清除
        error = 0;
        //设置巡航点停留时间(s)
        if( move_base_status != 0)
        {
            //alarm(move_base_timer);
            movebase_timer.start();

      ROS_WARN("will go to next cruise point:%d",move_base_timer);
            cout <<"准备前往下一个巡航点:"<<move_base_timer<<endl;
            cout << "--------------------------------------------------------------------" << endl;
        }
        //到达回充点，开启回充
        else if(go_charge_status == 1)
        {
            //on charge
            sleep(1);
            charge_control(1);
	
            ROS_WARN("going charge！");
            cout <<"开始回充！"<<endl;
            cout << "--------------------------------------------------------------------" << endl;
        }
    }
    //失败
    else if(result.status.status == 4)
    {
        //巡航再一次机会
        if(error == 0 && move_base_status != 0)
        {
            //一次重新的机会
            error = 1;
            if(mark_cruise_poit_array.markers.size())
            {
                x = mark_cruise_poit_array.markers.at(move_base_status).pose.position.x;
                y = mark_cruise_poit_array.markers.at(move_base_status).pose.position.y;
                z = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.z;
                w = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.w;
                set_goal("map",x,y,z,w);

                ROS_WARN("movebase error,on try");
                cout <<"巡航错误正在重试!"<<endl;
                cout << "--------------------------------------------------------------------" << endl;
            }
            else
            {
                ROS_WARN("have no cruise point");
                cout <<"未添加巡航点" << endl;
                cout << "--------------------------------------------------------------------" << endl;
            }
        }
        else if(move_base_status)
        {
            move_base_status = 0;
            error = 0;

            ROS_WARN("move_base error!");
            cout <<"巡航错误!"<<endl;
            cout << "--------------------------------------------------------------------" << endl;
        }

        //回充再一次机会
        if(error == 0 && go_charge_status != 0)
        {
            //一次重新的机会
            error = 1;
            goto_power_point();

            ROS_WARN("go charge error,on try");
            cout <<"回充错误正在重试!"<<endl;
            cout << "--------------------------------------------------------------------" << endl;
        }
        else if(go_charge_status)
        {
            go_charge_status = 0;
            error = 0;

            ROS_WARN("go charge error!");
            cout <<"回充错误!"<<endl;
            cout << "--------------------------------------------------------------------" << endl;
        }


       
    }
}


//导航取消函数
void move_base_cancel()
{
    actionlib_msgs::GoalID cancel_goal;
    movebase_cancel_pub.publish(cancel_goal);
    move_base_status = 0;
}

//发布导航目标点信息
void set_goal(std::string frame,double x,double y,double z,double w)
{
    geometry_msgs::PoseStamped goal;
    //设置frame
    goal.header.frame_id= frame;
    //设置时刻
    goal.header.stamp=ros::Time::now();
    goal.pose.position.x= x;
    goal.pose.position.y= y;
    goal.pose.position.z= 0;
    goal.pose.orientation.z= z;
    goal.pose.orientation.w= w;
    goal_pub.publish(goal);
    ros::spinOnce();
}

//timer out deal
void timer_handler(const ros::TimerEvent& e )
{
    double x,y,z,w;
    //巡航点停留时间到设置下一个巡航点
    if((move_base_status + 2) < mark_cruise_poit_array.markers.size())
    {
        move_base_status += 2;
        x = mark_cruise_poit_array.markers.at(move_base_status).pose.position.x;
        y = mark_cruise_poit_array.markers.at(move_base_status).pose.position.y;
        z = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.z;
        w = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.w;
        set_goal("map",x,y,z,w);
    }
    else
    {
        if(mark_cruise_poit_array.markers.size())
        {
          //循环
          move_base_status = 1;
          x = mark_cruise_poit_array.markers.at(move_base_status).pose.position.x;
          y = mark_cruise_poit_array.markers.at(move_base_status).pose.position.y;
          z = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.z;
          w = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.w;
          set_goal("map",x,y,z,w);
        }
        else
        {
      ROS_WARN("have no cruise point");
            cout << "未添加巡航点"<<endl;
            cout << "--------------------------------------------------------------------" << endl;
        }
    }

    movebase_timer.stop();

    ROS_WARN("go  timer !");
    cout << "--------------------------------------------------------------------" << endl;

}

//mode 0:off charger , 1:on charge
static void charge_control(u8 mode)
{
    std_msgs::UInt8 status;
    status.data = mode;
    go_charge_pub.publish(status);
    if(mode == 0)
    {
        go_charge_status = 0;
	
        ROS_WARN("exit charge !");
        cout << "--------------------------------------------------------------------" << endl;
    }
}

//mec底盘信息回调
void cmd_mecCallback(const ros_mec_msg::york &msg)
{
    std_msgs::Float32 power;
    power.data = msg.Power_Battery/10.0;
    power_pub.publish(power);

    static unsigned int low_power_cnt = 0,hight_power_cnt;
    static u8 time_cnt = 0;
    u8 charge_state = 0;
    static u8 go_charge_sucess_cnt = 0;

    //charge_state = (msg.State >> 8) & 0xff;


    double n = (msg.Power_Battery/10.0 - atof(min_power.c_str())) / (atof(max_power.c_str()) - atof(min_power.c_str()));
    int value = n*100;
    if(value > 100){value = 100;}

    //Low power----------------------------------------------------------------------------------------------------------------
    //如电量低于设置的回充电量且开启了低电量自动回充;持续多次低电量才算低电量,且在巡航模式下
    if(value < atof(go_power.c_str()) && atof(check_power.c_str()) != 0 && move_base_status != 0)
    {
        low_power_cnt ++;
        hight_power_cnt = 0;
	
        if((low_power_cnt % 5) == 0)
        {
          ROS_WARN("power is low will go charge !");
          cout << "电量低于:"<<go_power.c_str()<<"准备回充:"<<low_power_cnt<<endl;
          cout << "--------------------------------------------------------------------" << endl;
        }
        //回充
        if(low_power_cnt > 1000)
        {
            low_power_cnt = 0;
            //go charger point

            goto_power_point();

            ROS_WARN("low power start go charge !");
            cout << "--------------------------------------------------------------------" << endl;
           
        }
    }
    else
    {
        low_power_cnt = 0;
    }

    //如果电量高于95，且开启了低电量回充,且已经铜片接触,且不是因为低水量回充的
    if(value > 95 && atof(check_power.c_str()) != 0 && charge_state == 4)
    {
        hight_power_cnt++;
        low_power_cnt = 0;

        if((hight_power_cnt % 5) == 0)
        {
          ROS_WARN("power is high will exit charge !");
          cout << "电量高于:"<<go_power.c_str()<<"准备退出回充:"<<hight_power_cnt<<endl;
                cout << "--------------------------------------------------------------------" << endl;
        }
        
        //高电量重置回充标志
        if(hight_power_cnt > 1000)
        {
            hight_power_cnt = 0;
            charge_control(0);
        }
    }
    else
    {
        hight_power_cnt = 0;
    }


    switch(charge_state)
    {
        case 0:
        {
            break;
        }
        case 1:
        {
            break;
        }
        //error
        case 2:
        {
            if(go_charge_error_cnt == 0)
            {
                go_charge_error_cnt = 1;
            }
            
            charge_control(0);
            break;
        }
        case 3:
        {
            break;
        }
        //回充成功，正常接触
        case 4:
        {
            go_charge_sucess_cnt++;
            if(go_charge_sucess_cnt > 20)
            {
                go_charge_sucess_cnt = 0;
                ROS_WARN("go charge sucessful !");
                cout << "--------------------------------------------------------------------" << endl;
            }
            go_charge_error_cnt = 0;
            break;
        }
        //error
        case 5:
        {
            if(go_charge_error_cnt == 0)
            {
                go_charge_error_cnt = 1;
            }
            charge_control(0);
            break;
        }
        //error
        case 6:
        {
            if(go_charge_error_cnt == 0)
            {
                go_charge_error_cnt = 1;
            }
            charge_control(0);
            break;
        }
        case 7:
        {
            //回退完成后关闭回充
            if(msg.Vx == 0)
            {
                time_cnt++;
            }
            else
            {
                time_cnt = 0;
            }

            if(time_cnt > 10)
            {
                time_cnt = 0;
                charge_control(0);

                if(go_charge_temp_movebase_status != 0)
                {
                    double x,y,z,w;
                    move_base_status = go_charge_temp_movebase_status;
                    x = mark_cruise_poit_array.markers.at(move_base_status).pose.position.x;
                    y = mark_cruise_poit_array.markers.at(move_base_status).pose.position.y;
                    z = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.z;
                    w = mark_cruise_poit_array.markers.at(move_base_status).pose.orientation.w;
                    set_goal("map",x,y,z,w);
                    go_charge_temp_movebase_status = 0;
                }
                
            }
            break;
        }
        default:break;
    }

    if(charge_state == 4)
    {
         go_charge_sucess_flag = 1;
    }
    else
    {
         go_charge_sucess_flag = 0;
    }

    if(go_charge_error_cnt == 1)
    {
        //回退完成后关闭回充
        if(msg.Vx == 0)
        {
            time_cnt++;
        }
        else
        {
            time_cnt = 0;
        }

        if(time_cnt > 10)
        {
            go_charge_error_cnt = 2;
            goto_power_point();
	    
           ROS_WARN("error go powerpoint !");
            cout << "--------------------------------------------------------------------" << endl;
            
        }
    }
    /*
    if()
    {
        cout << "charge State:"<<msg.State<<endl;
        cout << "--------------------------------------------------------------------" << endl;
    }*/
    //cout << "power:"<<value <<endl;
    //emit power(message_holder.data);
}

//power point write in txt
void powerPointSet(void)
{
    std::string str= file_pwd + "/ros_robot_control/pose/power_pose.txt";
    const char *p = str.c_str();//同上，要加const或者等号右边用char*
    //write in txt
    std::ofstream out(p,std::ios::out);
    
    out<<fixed<<setprecision(2)<<TempPower_point(0)<<"\t\t"<<TempPower_point(1)<<"\t\t"<<TempPower_point(2)<<std::endl; 
    out.close();

    geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(TempPower_point(2));


    // Set the frame ID and timestamp.  See the TF tutorials for information on these.
    powerpointmark.header.frame_id = "map";
    powerpointmark.header.stamp = ros::Time::now();

    // Set the namespace and id for this marker.  This serves to create a unique ID
    // Any marker sent with the same namespace and id will overwrite the old one
    powerpointmark.ns = "basic_shapes";
    powerpointmark.id = 0;

    // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
    powerpointmark.type = visualization_msgs::Marker::CUBE;

    // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
    powerpointmark.action = visualization_msgs::Marker::ADD;

    // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header

    powerpointmark.pose.position.x = TempPower_point(0);
    powerpointmark.pose.position.y = TempPower_point(1);
    powerpointmark.pose.position.z = 0;
    powerpointmark.pose.orientation.x = 0.0;
    powerpointmark.pose.orientation.y = 0.0;
    powerpointmark.pose.orientation.z = goal_quat.z;
    powerpointmark.pose.orientation.w = goal_quat.w;

    // Set the scale of the marker -- 1x1x1 here means 1m on a side
    powerpointmark.scale.x = 0.3;
    powerpointmark.scale.y = 0.5;
    powerpointmark.scale.z = 0.3;

    // Set the color -- be sure to set alpha to something non-zero!
    powerpointmark.color.r = 0.0f;
    powerpointmark.color.g = 1.0f;
    powerpointmark.color.b = 0.0f;
    powerpointmark.color.a = 1.0;

    //time 永久
    powerpointmark.lifetime = ros::Duration();

    powerpoint_pub.publish(powerpointmark);
}

//机器人位置话题的回调函数
void poseCallback(const geometry_msgs::PoseWithCovarianceStamped& pos)
{
    double pose_th = 0.0;
    tf::Quaternion q;

    tf::quaternionMsgToTF(pos.pose.pose.orientation, q);
    pose_th = tf::getYaw(q);  //角度
    //TempPower_point(0) = pos.pose.pose.position.x;
    //TempPower_point(1) = pos.pose.pose.position.y;
    //TempPower_point(2) = pose_th;
}

//
//第一次和TF上获得位置信息
void update_pose(void)//定义更新函数
{
    double pose_th = 0.0;
    tf::Quaternion q;
    geometry_msgs::Pose current_pose_ros;
    tf::StampedTransform transform;
    tf::TransformListener listener;

    try 
    {
        //得到坐标odom和坐标base_link之间的关系
        listener.waitForTransform("map","base_footprint", ros::Time(0), ros::Duration(3));
        listener.lookupTransform("map","base_footprint",ros::Time(0), transform);

    } 
    catch (std::exception e ) 
    {
        ROS_ERROR("move base cannot get robot pose!");
        return;
    }

    geometry_msgs::TransformStamped transform_pose;
    tf::transformStampedTFToMsg(transform, transform_pose);
    current_pose_ros.position.x = transform.getOrigin().x();
    current_pose_ros.position.y = transform.getOrigin().y();
    current_pose_ros.position.z = 0.0;
    current_pose_ros.orientation= transform_pose.transform.rotation;

    tf::quaternionMsgToTF(current_pose_ros.orientation, q);
    pose_th = tf::getYaw(q);  //角度

    TempPower_point(0) = current_pose_ros.position.x;
    TempPower_point(1) = current_pose_ros.position.y;
    TempPower_point(2) = pose_th;

}

//===============================================================
// 语法格式：    void main(void)
// 实现功能：    主函数，建立一个TCP并发服务器
// 入口参数：    无
// 出口参数：    无
//===============================================================
int main(int argc, char *argv[])
{
    ros::init(argc, argv, "ros_robot_control_node");
    ros::NodeHandle n;
    std_msgs::UInt8 status;
    unsigned int time_cnt = 0;

    //timer
    movebase_timer = n.createTimer(ros::Duration(1.0), timer_handler,false);
    movebase_timer.stop();


    robot_control_sub = n.subscribe("/robot_control", 10, robot_control_Callback);
     //设置巡航点
    cruise_point_sub = n.subscribe("set_cruise",1000,cruisepointCallback);
    //movebase result
    movebase_result_sub = n.subscribe("move_base/result",10,movebase_resultCallback);
     //机器人位置话题
    pos_sub = n.subscribe("/amcl_pose",1000,poseCallback);
    //电压订阅和回充状态订阅
    zkwl_robot_sub = n.subscribe("/mec_car", 1,cmd_mecCallback);


    //导航目标点发送话题
    goal_pub = n.advertise<geometry_msgs::PoseStamped>("move_base_simple/goal",1000);
    //底盘状态
    robot_status_pub   = n.advertise<std_msgs::UInt8>("robot_control_status", 1000);    //告诉系统要发布话题了
    //巡航点设置发布
    markarray_pub = n.advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 1000);
    //导航取消
    movebase_cancel_pub = n.advertise<actionlib_msgs::GoalID>("move_base/cancel", 1);
     //充电点位置发布
    powerpoint_pub = n.advertise<visualization_msgs::Marker>("power_marker", 1000);
    //回充控制
    go_charge_pub  = n.advertise<std_msgs::UInt8>("go_charge", 100);
    //电压发布
    power_pub = n.advertise<std_msgs::Float32>("power", 100);

    printf("start to ros_robot_control_node\n");


    const char *file_name = "src";
    char abs_path_buff[100];
    //获取文件路径, 填充到abs_path_buff
    //realpath函数返回: null表示获取失败; 否则返回指向abs_path_buff的指针
    if(realpath(file_name, abs_path_buff))
    {
        //文件绝对路径赋值
        file_pwd = abs_path_buff;
        cout << file_pwd <<endl;
        cout << "--------------------------------------------------------------------" << endl;
    }
    else
    {
	ROS_ERROR("the file src is not exist");
        cout << "--------------------------------------------------------------------" << endl;   
    }

    //load mark
    load_mark_frompostxt();
    //load powerpointmark
    Load_power_markfromtxt();
    //load goto power value
    Load_goto_power_valuefromyaml();

    ros::Rate loop_rate(10);    //设置发送数据的频率为10Hz
    while(ros::ok())
    {
        
        time_cnt++;
        //1hz pub status
        if(time_cnt > 10)
        {
            time_cnt = 0;
            status.data = 0;
            //zkwlrobot
            if(getProcessPidByName("ros_mec_odom") == -1)
            {

            }
            else
            {
                status.data |= 0x01;
            }

            //movebase
            if(getProcessPidByName("move_base") == -1)
            {
                //statdata["zkwl_robot"] = "None";
            }
            else
            {
                //statdata["zkwl_robot"] = "Run";
                //pub mark array
                markarray_pub.publish(mark_cruise_poit_array);
                powerpoint_pub.publish(powerpointmark);
                status.data |= 0x02;
            }

            //karto
            if(getProcessPidByName("slam_karto") == -1)
            {
                //statdata["slam_karto"] = "None";

            }
            else
            {
                //statdata["slam_karto"] = "Run";
                status.data |= 0x04;
            }

            if(go_charge_status  || go_charge_sucess_flag)
            {
                //doing go charge
                status.data |= 0x08;
            }
            
            //pub status
            robot_status_pub.publish(status);   //向话题“str_message”发布消息


        }
        
        ros::spinOnce();    //不是必须，若程序中订阅话题则必须，否则回掉函数不起作用。
        loop_rate.sleep();  //按前面设置的10Hz频率将程序挂起
     }
    ros::shutdown();
    pthread_detach(thread_zkwl_robot); // 线程分离，结束时自动回收资源
    pthread_detach(thread_move_base); // 线程分离，结束时自动回收资源
    pthread_detach(thread_slam_karto); // 线程分离，结束时自动回收资源
    return 0;
}
